#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy
from rclpy.node import Node
import tkinter as tk
from tkinter import messagebox
from PIL import Image, ImageTk
import threading
import imageio # type: ignore
import os
import sys
import moveit_commander
import faulthandler
from all_path import run_selected_trajectory
from all_fc import force_main, stop_force_control, run_force_control
from all_eih import visual_main, stop_visual_control, run_visual_control
from all_path2 import execute_path2
from all_eih_once import visual_once, run_visual_control2, stop_visual_control2
from all_fc_once import force_once
from mqtt_integration.msg import TrajectoryInfo ,TimeInfo

# TrajectoryPublisher 类定义
class TrajectoryPublisher:
    def __init__(self):
        # 定义发布器（可根据需要发布轨迹信息）
        self.pub = rospy.Publisher('trajectory_info', TrajectoryInfo, queue_size=10)
        # 定义订阅器，订阅 ROS 中的 time_info 话题
        self.sub = rospy.Subscriber('time_info', TimeInfo, self.time_info_callback)
        self.current_time_hex = "未接收到"
        self.elapsed_time_str = "未接收到"
        # 每 2 秒调用一次 publish_info_timer（可以扩展为定期发布轨迹信息）
        rospy.Timer(rospy.Duration(2), self.publish_info_timer)

    def time_info_callback(self, msg):
        # 保存接收到的 time_info 消息
        self.current_time_hex = msg.current_time
        self.elapsed_time_str = msg.elapsed_time

    def get_time_info(self):
        # 提供获取时间信息的方法
        return self.current_time_hex, self.elapsed_time_str

    def publish_info_timer(self, event):
        self.publish_info()

    def publish_info(self):
        # 如果需要发布其他信息，可以实现这个函数
        pass

# """重启当前程序"""
def restart_program():
    python = sys.executable # 获取 Python 解释器的位置
    os.execl(python, python, *sys.argv) # 使用解释器重新运行代码

# 定义一个函数定期更新时间信息
def update_time_label():
    # 获取时间信息并更新到 time_label 上
    current_time_hex, elapsed_time_str = demo.get_time_info()
    time_label.config(text=f"当前时间: {current_time_hex}\n运行时间: {elapsed_time_str}")

    # 每秒更新一次时间
    root.after(1000, update_time_label)

# 初始化ROS节点
rospy.init_node('move_group_python_interface', anonymous=True)

# 初始化Tkinter窗口
root = tk.Tk()
root.title("喂食机控制面板")
root.geometry("700x600")

# 创建 TrajectoryPublisher 实例
demo = TrajectoryPublisher()

def start_eating():
    status_label.config(text="当前状态: 正在喂食")
    # error_label.config(text="错误信息: 无")

def show_gif(file_path):
    global gif_label, gif_frames, gif_index, gif_animation

    # 停止当前正在播放的GIF动画
    if 'gif_animation' in globals():
        gif_label.after_cancel(gif_animation)

    gif_frames = imageio.mimread(file_path)
    gif_index = 0  # 从第一帧开始播放

    def update_gif():
        global gif_index, gif_animation
        if root.winfo_exists():  # 检查Tkinter窗口是否还在运行
            frame_image = ImageTk.PhotoImage(Image.fromarray(gif_frames[gif_index]).resize((260, 200), Image.Resampling.LANCZOS))
            gif_label.config(image=frame_image)
            gif_label.image = frame_image  # 保持引用
            gif_index = (gif_index + 1) % len(gif_frames)  # 更新索引
            gif_animation = gif_label.after(100, update_gif)  # 动画效果
        else:
            gif_label.after_cancel(gif_animation)  # 如果窗口关闭，则取消动画

    update_gif()

def show_image(file_path):
    image = Image.open(file_path)
    image = image.resize((260, 260), Image.Resampling.LANCZOS)
    img = ImageTk.PhotoImage(image)
    gif_label.config(image=img)
    gif_label.image = img  # 保持引用

def update_status_and_gif(trajectory_number):
    if trajectory_number == 1:
        status_label.config(text="当前状态: 工作")
        path_label.config(text="取食路径: 一")
        show_gif("/home/jetson/hhq/轨迹1.gif")
    elif trajectory_number == 2:
        status_label.config(text="当前状态: 工作")
        path_label.config(text="取食路径: 二")
        show_gif("/home/jetson/hhq/轨迹2.gif")
    elif trajectory_number == 3:
        status_label.config(text="当前状态: 工作")
        path_label.config(text="取食路径: 三")
        show_gif("/home/jetson/hhq/轨迹3.gif")
    else:
        reset_status()

def reset_status():
    global gif_animation
    if 'gif_animation' in globals():
        gif_label.after_cancel(gif_animation)
    status_label.config(text="当前状态: 空闲")
    path_label.config(text="取食路径: 无")
    show_image("/home/jetson/hhq/home.png") 

# 喂食速度列表和当前索引
speed_options = ["中", "快", "慢"]
current_speed_index = 0

def toggle_speed(event=None):  
    global current_speed_index
    current_speed_index = (current_speed_index + 1) % len(speed_options)
    path_set_label.config(text=f"喂食速度:{speed_options[current_speed_index]}")

def get_speed():
    # 根据当前速度索引返回速度字符串
    if current_speed_index == 0:
        return 'medium'
    elif current_speed_index == 1:
        return 'fast'
    else:
        return 'slow'

# 定义一个全局变量，用于记录是否有任务在执行
task_running = False

def execute_trajectory_by_button(traj_index):
    global task_running
    if task_running:
        messagebox.showwarning("警告", "已有任务在执行，请稍后再试")
        return

    # 设置任务正在执行
    task_running = True
    
    # 更新状态和GIF
    update_status_and_gif(traj_index)
    # 使用线程执行轨迹，以保持界面的响应
    threading.Thread(target=run_trajectory_in_thread, args=(traj_index,)).start()

def run_trajectory_in_thread(traj_index):
    global task_running
    try:
        # 执行轨迹并设置速度
        speed = get_speed()
        print(f"执行取食路径 {traj_index}，速度设置为: {speed}")
        run_selected_trajectory(traj_index, speed)
    except Exception as e:
        print(f"执行轨迹时出错: {e}")
        # error_label.config(text=f"错误信息: {e}")
    finally:
        reset_status()  # 状态重置
        task_running = False  # 任务标记恢复到False，允许新的任务
        print("流程执行完毕。")

def execute_force_control():
    try:
        # 使用线程执行力控操作，以保持界面的响应
        threading.Thread(target=run_force_control_in_thread).start()
    except Exception as e:
        messagebox.showerror("错误", f"执行失败: {e}")

def run_force_control_in_thread():
    try:
        force_main()
    except Exception as e:
        print(f"执行力控时出错: {e}")

def execute_visual_control():
    try:
        # 使用线程执行力控操作，以保持界面的响应
        threading.Thread(target=run_visual_control_in_thread).start()
    except Exception as e:
        messagebox.showerror("错误", f"执行失败: {e}")

def run_visual_control_in_thread():
    try:
        visual_main()
    except Exception as e:
        print(f"执行力控时出错: {e}")

def execute_all_process():
    try:
        # 使用线程执行力控操作，以保持界面的响应
        threading.Thread(target=run_all_process_in_thread).start()
    except Exception as e:
        messagebox.showerror("错误", f"执行失败: {e}")

def run_all_process_in_thread():
    try:
        execute_path2()
        visual_once()
        rospy.sleep(8)
        force_once()
    except Exception as e:
        print(f"执行力控时出错: {e}")

def kill_process():
    stop_force_control()
    stop_visual_control()
    stop_visual_control2()
    # 初始化 moveit_commander
    group = moveit_commander.MoveGroupCommander("manipulator")
    group.set_named_target("middle")
    plan = group.go(wait=True)
    group.stop()
    group.clear_pose_targets()
    print("机械臂已移动到 middle 位置。")
    run_force_control()
    run_visual_control()
    run_visual_control2()

# 右上角时间显示（通过ROS主题更新）
time_label = tk.Label(root, text="当前时间: 未接收到\n经过时间: 未接收到", anchor="w", justify="left")
time_label.place(x=400, y=10)

# 当前状态
status_label = tk.Label(root, text="当前状态: 空闲", borderwidth=5, relief="groove", font=("Helvetica", 30), padx=25, pady=15)
status_label.place(x=50, y=50)

# 食物路径
path_label = tk.Label(root, text="取食路径: 无", borderwidth=5, relief="groove", font=("Helvetica", 30), padx=45, pady=15)
path_label.place(x=50, y=155)

# # 错误信息
# error_label = tk.Label(root, text="错误信息: 无", borderwidth=5, relief="groove", font=("Helvetica", 30), padx=45, pady=20)
# error_label.place(x=50, y=350)

# 喂食速度
path_set_label = tk.Label(root, text=f"喂食速度:{speed_options[current_speed_index]}", borderwidth=5, relief="groove", font=("Helvetica", 30), padx=50, pady=15)
path_set_label.place(x=50, y=260)
path_set_label.bind("<Button-1>", toggle_speed)  # 绑定点击事件

force_button = tk.Button(root, text="力控", borderwidth=2, font=("Helvetica", 30), padx=35, pady=15, command=execute_force_control)
force_button.place(x=50, y=365) 

visual_button = tk.Button(root, text="视觉", borderwidth=2, font=("Helvetica", 30), padx=35, pady=15, command=execute_visual_control)
visual_button.place(x=210, y=365)  

home_button = tk.Button(root, text="HOME", borderwidth=2, font=("Helvetica", 30), padx=35, pady=15, command=kill_process)
home_button.place(x=50, y=470) 

visual_button = tk.Button(root, text="流程", borderwidth=2, font=("Helvetica", 30), padx=35, pady=15, command=execute_all_process)
visual_button.place(x=210, y=470) 

visual_button = tk.Button(root, text="reset", borderwidth=2, font=("Helvetica", 15), padx=20, pady=8, command=restart_program)
visual_button.place(x=570, y=8) 

# 机器图片占位符
gif_label = tk.Label(root, borderwidth=5, relief="groove", width=260, height=200)
gif_label.place(x=380, y=60)

# 初始化时显示空闲状态的图片
reset_status()

# 按钮绑定到执行不同轨迹的函数
start_button = tk.Button(root, text="取食路径一", font=("Helvetica", 20), padx=30, pady=20, command=lambda: execute_trajectory_by_button(1))
start_button.place(x=440, y=300)

speed_button = tk.Button(root, text="取食路径二", font=("Helvetica", 20), padx=30, pady=20, command=lambda: execute_trajectory_by_button(2))
speed_button.place(x=440, y=405)

path_button = tk.Button(root, text="取食路径三", font=("Helvetica", 20), padx=30, pady=20, command=lambda: execute_trajectory_by_button(3))
path_button.place(x=440, y=510)

# 启动Tkinter时启动时间信息更新
update_time_label()

# 启动Tkinter主循环
root.mainloop()
